#pragma once

#include <ros/ros.h>
#include <ros/master.h>
#include <cstdlib>
#include <std_msgs/String.h>
#include <std_msgs/Int8.h>
#include <std_msgs/Bool.h>
#include <kybot_msg/SystemEvent.h>
#include <kybot_msg/StateNotify.h>
#include <kybot_msg/exceptionNotifySrv.h>
#include "kybot_msg/StateNotify.h"
#include <kybot_msg/Exceptiontonavigation.h>
#include <ros/package.h>
#include <boost/filesystem.hpp>
#include <nlohmann/json.hpp>
#include <fstream>
#include <algorithm>
#include <atomic>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/CameraInfo.h>
#include <bosch_locator_bridge/ClientControlMode.h>
#include <kybot_msg/TargetPoseNavigationControlSrv.h>
#include <kybot_msg/BaseInfo.h>
#include <kybot_msg/NodeInfoSrv.h>
#include <kybot_msg/stopLocationSrv.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <glog/logging.h>
#include <unordered_map>
#include <stdio.h>
#include <iostream>
#include <string.h>
#include <vector>
#include <sensor_msgs/Imu.h>
#include <nav_msgs/Odometry.h>


//异常分为4种，无异常，一般异常，致命异常
enum exceptionType
{
    NOEXCEPTION,
    WARNING,
    ERROR,
    FATAL
};

class RobotException
{
public:
    RobotException();

    void initialize();
    void run();
    void exceptionNotifyCallback(const kybot_msg::StateNotify::ConstPtr &msg);
    void McusendCallback(const std_msgs::String::ConstPtr &msg);
    bool exceptionNotifySrvCallback(kybot_msg::exceptionNotifySrv::Request &req, kybot_msg::exceptionNotifySrv::Response &res);
    void navigationResultCallback(const kybot_msg::SystemEvent::ConstPtr &result);
    void locationsoftCallback(const bosch_locator_bridge::ClientControlModeConstPtr& msg);
    void locationposeCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg);
    void BaseInfoCallback(const kybot_msg::BaseInfoConstPtr &msg);
    bool navigation_tostop();
    bool stopLocationSrvcallback(kybot_msg::stopLocationSrv::Request &req, 
							kybot_msg::stopLocationSrv::Response &res);
    void HardwareCheck();
    void nodecheck();
    void locationcheck();
    void CommunicationCheck();
    void locationdriftCallback(const std_msgs::Bool::ConstPtr &msg);
    void McuerrorCallback(const std_msgs::String::ConstPtr &msg);
    template <typename M>
    void HardwareCallback(M m, bool* b) {
        if (nullptr == b)
            return;
        *b = true;
    }

private:
    void InitGlog(const std::string& log_dir, const std::string& log_name);
    bool readExceptionList();
    int getExceptionType();
    void AddErrorToErrorlist(const std::string& error);

    ros::NodeHandle nh_;
    ros::Subscriber exceptionNotifySuber_,navigationResultSuber_;
    ros::Subscriber frontlaserSuber_,backlaserSuber_,rightlaserSuber_,frontcameraSuber_,backcameraSuber_;
    ros::Subscriber odom_suber_,imu_suber_;
    ros::Subscriber Location_soft_,Location_pose_,Location_driftSuber_;
    ros::Subscriber baseinfo_sub_;
    ros::Subscriber mcusend_sub_;
    ros::Subscriber mcuerror_sub_;
    ros::Publisher exceptionTypePuber_,exception_taskPuber_;
    ros::ServiceServer exceptionNotifySrv_;
    ros::ServiceServer stop_location_srv_;
    
    exceptionType exceptionType_;
    std::vector<std::string> exceptionList_,lastList_,mcucheckList_;
    std::string navigationType_;
    std::string navigationResultNotify_;
    std::vector<std::string> normalList_,fatalList_;

    //硬件检查
    //软件检查
    //通信检查
    //节点检查
    std::string systemStatusTopic_,frontLaserTopic_,backLaserTopic_,rightLaserTopic_,odomTopic_,imuTopic_,frontCameraTopic_,backCameraTopic_,locationSoftTopic_,locationPoseTopic_;
    std::string locationDriftTopic_,baseInfoTopic_,mcuTimeOutTopic_,mcuErrorTopic_;
    bool checkSystemStatus_,checkFrontLaser_,checkBackLaser_,checkRightLaser_,checkFrontCamera_,checkBackCamera_,checkOdom_,checkImu_,checkLocationSoft_,checkLocationPose_,checkLocationDrift_,checkBaseInfo_,checkMcuTimeOut_,checkMcuError_;

    std::string frontLaserNode_,backLaserNode_,rightLaserNode_,frontCameraNode_,backCameraNode_,odomNode_,ImuNode_,interfaceNode_,systemNode_,navigationNode_,autoChargeNode_,perceptionNode_,mapNode_;
    std::string accuratePositionNode_,writeBagNode_,locationSoftNode_,locationSoftServerNode_,mcuNode_,joyNode_,joyControlNode_,autoDeleteNode_;
    bosch_locator_bridge::ClientControlMode Location_msg;
    bool location_recv_;
    bool stopLocation_;
    bool location_nodata;
    bool locationdrift_;

    //
    std::map<std::string, bool> hardware_check_map_;
    using json = nlohmann::json;
    json data;
};